/*
 * Utility.cpp

 *
 *  Created on: 03/ott/2013
 *      Author: escion
 */
#include<iostream>
#include<arpa/inet.h>
#include<unistd.h>
#include<sys/socket.h>
#include<sys/types.h>
#include<stdio.h>
#include<string.h>
#include<stdlib.h>
#include<opencv2/opencv.hpp>
#include<opencv2/features2d/features2d.hpp>
#include<opencv2/highgui/highgui.hpp>
#include "Utility.h"
using namespace std;
using namespace cv;

//Order nodes by decreasing capacity link (bubble sort)
void orderNodes(int clientsNumber, list_nodes nodeList[])
{
	list_nodes support;
	for(int i=1;i<clientsNumber+1;i++)
	{
		for(int k=1;k<(clientsNumber+1)-i;k++)
		{
			if(nodeList[k].bandwidth<nodeList[k+1].bandwidth)
			{
				//swapping
				cout<<"Swapping"<<endl;
				memcpy(&support,&nodeList[k],sizeof(nodeList[k]));
				memcpy(&nodeList[k],&nodeList[k+1],sizeof(nodeList[k]));
				memcpy(&nodeList[k+1],&support,sizeof(nodeList[k]));
			}
		}
	}
}
uchar* createInfoPacket(uchar* packet, int cols, int rows, int blocks)
{
	int i=0;
	packet[i]=(cols>>24);
	packet[i++]=(cols>>16);
	packet[i++]=(cols>>8);
	packet[i++]=(cols);
	packet[i++]=(rows>>24);
	packet[i++]=(rows>>16);
	packet[i++]=(rows>>8);
	packet[i++]=(rows);
	packet[i++]=(blocks>>24);
	packet[i++]=(blocks>>16);
	packet[i++]=(blocks>>8);
	packet[i++]=(blocks);
	//for(int k=0;k<12;k++)
	//	printf("%d ",packet[k]);
	//printf("\n");
	return packet;
}
//create broadcast packet (with header serialization)
void createHeader(broadcast_packet packet[],int payloadDimension, int packetNumber)
{
	packet[packetNumber-1].data[0]=(payloadDimension >> 24);
	packet[packetNumber-1].data[1]=(payloadDimension >> 16);
	packet[packetNumber-1].data[2]=(payloadDimension >> 8);
	packet[packetNumber-1].data[3]=(payloadDimension);
	packet[packetNumber-1].data[4]=(packetNumber >> 24);
	packet[packetNumber-1].data[5]=(packetNumber >> 16);
	packet[packetNumber-1].data[6]=(packetNumber >> 8);
	packet[packetNumber-1].data[7]=(packetNumber);
}

int deserializeSpeed(char speed[5])
{
	int CPUspeed;
	CPUspeed = (speed[1] << 24);
	CPUspeed = CPUspeed + (speed[2] << 16);
	CPUspeed = CPUspeed + (speed[3] << 8);
	CPUspeed = CPUspeed + (speed[4]);
	return CPUspeed;
}
//Print error
void printErrorLog(string s)
{
	cout << s << endl;
	exit(1);
}
//Parsing iperf log file to get the bandwidth
float getBandwidth(bool udp)
{
	float bandwidth;
	FILE* file = fopen("/home/ubuntu/iperfLog.txt","r");
	if(file == NULL)
		return 0;
	//Direct access at the 352nd byte
	if(!udp)
		fseek(file,352,0);
	else
		fseek(file,380,0);
	//Reading the bandwidth value
	fscanf(file,"%f",&bandwidth);
	fclose(file);
	return bandwidth;
}

void computeLoadUnicast(int clientsNumber, list_nodes nodeList[],int o, int loads)
{
	if(loads==1)
	{
		if(clientsNumber==1)
		{
			nodeList[0].load = floor((100-o)/2);
			cout<<"a0: "<<nodeList[0].load<<endl;
			nodeList[1].load = 100-nodeList[0].load;
			cout<<"a1: "<<nodeList[1].load<<endl;
		}
		else if(clientsNumber==2)
		{
			int v[3];
			float h=1.015;
			for(int i=0;i<clientsNumber+1;i++)
				v[i]=5000000;
			nodeList[0].load = (100-o*(1+h*(v[1]/nodeList[2].bandwidth)))/(3+h*(v[1]/nodeList[2].bandwidth));
			nodeList[2].load = nodeList[0].load+o;
			nodeList[1].load = 100 - nodeList[0].load - nodeList[2].load;
			cout<<"a0: "<<nodeList[0].load<<endl;
			cout<<"a1: "<<nodeList[1].load<<endl;
			cout<<"a2: "<<nodeList[2].load<<endl;
		}
		else if(clientsNumber==3)
		{
			//matlab
		}
		else if(clientsNumber==4)
		{
			//matlab
		}
	}

	else if(clientsNumber==1 && loads ==0)
	{
		nodeList[0].load=25;
		nodeList[1].load=75;
	}
	else if(clientsNumber==2 && loads ==0)
	{
		cout<<"Compute loads"<<endl;
		nodeList[0].load=27;
		nodeList[1].load=37;
		nodeList[2].load=36;
	}
	else if(clientsNumber==2 && loads==1)
	{

	}
}

void computeLoadBroadcast(int clientsNumber,list_nodes nodeList[],int o)
{
	int b[clientsNumber+1];
	int sumCPUspeed=0,i;
	nodeList[0].CPUspeed = 5000000;
	for(i=0;i<(clientsNumber+1);i++)
		sumCPUspeed+=nodeList[i].CPUspeed;
	b[0]= (int)(((100+clientsNumber*o)*nodeList[0].CPUspeed)/(sumCPUspeed));
	nodeList[0].load = b[0]-o;
	for(i=1;i<clientsNumber+1;i++)
	{
		b[i]=(nodeList[i].CPUspeed/nodeList[0].CPUspeed)*b[0];
		if(i==clientsNumber)
			nodeList[i].load=b[i];
		else
			nodeList[i].load=b[i]-o;
	}
	for(int i=0;i<clientsNumber+1;i++)
		cout<<"a"<<i<<": "<<nodeList[i].load<<endl;
}

//Find maximum processing time
double computeMaximumProcessingTime(int clientsNumber, list_nodes nodeList[])
{
	int k;
	double max;
	for(k=0,max=0;k<clientsNumber+1;k++)
	{
		if(nodeList[k].time>max)
			max=nodeList[k].time;
	}
	return max;
}
//Check keypoints and descriptors
int check(int clientsNumber, list_nodes nodeList[], vector<KeyPoint> kp, Mat descriptors)
{
	int k=0;
	/*for(int i=0;i<kp.size();i++,k=k+2)
	{
		printf("x: %f y: %f x:%f y:%f\n",kp[i].pt.x,kp[i].pt.y,nodeList[0].keypoints[k],nodeList[0].keypoints[k+1]);
	}
	k=0;
	for(int i=0;i<kp.size();i++,k=k+2)
	{
		printf("x: %f y: %f x:%f y:%f\n",kp[i].pt.x,kp[i].pt.y,nodeList[1].keypoints[k],nodeList[1].keypoints[k+1]);
	}*/
	//Keypoints size check
	vector<float> differentKeypoints;
	int sum,j,count = 0, sameKp = 0, differentKp = 0,overlap=0,sameDescr = 0,differentDescr = 0,i;
	int support;
	for(k=0,sum=0;k<clientsNumber+1;k++)
		sum += nodeList[k].keypointsDetected;
	if(sum<kp.size())
		return 1;
	cout<<"Sum without checking the content: "<<sum<<endl;
	bool found = false;
	//for each keypoint computed only by the camera
	vector<int> index;
	for(count=0;count<kp.size();count++)
	{
		//for each node (including the camera)
		for(k=0;k<(clientsNumber+1) && !found;k++)
		{
			//Add the height of the previous image (relative coordinates)
			if(k>0)
			{
				for(int h=0;h<clientsNumber;h++)
					overlap = overlap + nodeList[h].height;
			}
			//For each keypoint computed by a node
			for(j=0;j<nodeList[k].keypointsDetected*2 && !found;j=j+2)
			{
				//Check if the coordinates lie in a certain range
				if(kp[count].pt.x >= nodeList[k].keypoints[j]-0.5
					&& kp[count].pt.x <= nodeList[k].keypoints[j]+0.5
					&& kp[count].pt.y >= nodeList[k].keypoints[j+1]+overlap-0.5
					&& kp[count].pt.y <= nodeList[k].keypoints[j+1]+overlap+0.5)
				{
					found=true;
					//Local index
					index.push_back(count);
					//Node k
					index.push_back(k);
					//Distributed descriptor index
					index.push_back(j);
					//printf("SAME KEYPOINT. x: %f x: %f y: %f y: %f\n",kp[count].pt.x,nodeList[k].keypoints[j],kp[count].pt.y,nodeList[k].keypoints[j+1]+overlap);
					//printf("Angle: %f size: %f\n",kp[count].angle,kp[count].size);
					//cout<<"count: "<<count<<" k: "<<k<<" j: "<<j<<endl;
					sameKp++;
				}
			}
		}
		if(!found)
		{
			//cout<<"Different keypoint. x: "<<kp[count].pt.x<<" y: "<<kp[count].pt.y<<endl;
			differentKp++;
		}
		found=false;
		overlap=0;
	}
	cout<<"Same keypoints: "<<sameKp<<endl;
	cout<<"Different keypoints: "<<differentKp<<endl;
	found=false;
	cout<<"Bitwise descriptors check"<<endl;
	//CHECK BITWISE
	//Checking the descriptors of the same keypoints
	int lIndex,node,dIndex,counter = 0,aux;
	uchar a,b;
	vector<int> differentBits;
	for(i=0;i<index.size();i=i+3)
	{
		lIndex = index[i]*64;
		node = index[i+1];
		dIndex = (index[i+2]*64)/2;
		//cout<<"Descriptor: "<<lIndex<<" node: "<<node<<" distributed: "<<dIndex<<endl;
		for(k=0;k<64;k++)
		{
			a = descriptors.data[lIndex+k];
			b = nodeList[node].descriptors[dIndex+k];
			/*if(i==0)
			{
				aux = compareBytes(a,b);
				printf("a: %d b: %d different bits: %d ",a,b,aux);
				counter = counter + aux;
				cout<<"counter: "<<counter<<endl;
			}
			else*/
			counter = counter + compareBytes(a,b);
		}
		//cout<<"Different bits: "<<counter<<endl;
		differentBits.push_back(counter);
		counter = 0;
	}
	sum = 0;
	for(i=0;i<differentBits.size();i++)
	{
		sum = sum + differentBits[i];
		printf("%d ",differentBits[i]);
	}
	printf("\n");
	printf("%f\n",(float)sum/differentBits.size());
	//cout<<"Averaged different bits: "<<sum/differentBits.size()<<endl;

	/*
	//CHECK BYTEWISE
	int support=0;
	for(count=0;count<kp.size()*64;count=count+64)
	{
		//cout<<"Checking descriptor: "<<count<<endl;
		for(k=0;k<clientsNumber+1 && !found;k++)
		{
			//cout<<"Node: "<<k<<endl;
			for(j=0,support=0;j<nodeList[k].descriptorsSize && !found;j++)
			{
				//cout<<(int)descriptors.data[count+h]<<" "<<(int)nodeList[k].descriptors[j]<<endl;
				if((int)descriptors.data[count+support]==(int)nodeList[k].descriptors[j])
				{
					support++;
					if((j+1)%64==0)
					{
						//cout<<"SAME DESCRIPTOR. Node: "<<k<<" Start: "<<(j-63)/64<<" Server side initial count: "<<count/64<<endl;
						sameDescr++;
						found=true;
						support=0;
						//startCheck=j+1;
					}
				}
				else
				{
					//cout<<"Different byte: "<<j<<endl;
					j=j-1-support+64;
					//cout<<"New index: "<<j<<endl;
					support=0;
				}
			}
		}
		if(!found)
			differentDescr++;
		found=false;
	}
	cout<<"Same descriptors: "<<sameDescr<<endl;
	cout<<"Different descriptors: "<<differentDescr<<endl;
	if(sameKp<kp.size())
		return 2;

	/*for(k=0;k<clientsNumber+1;)
	{
		cout<<"Node: "<<k<<" keypoints: "<<nodeList[k].keypointsDetected<<endl;
		cout<<"Overlap: "<<overlap<<endl;
		//Checking keypoints
		cout<<"CHECKING KEYPOINTS"<<endl;
		//check tra lista solo server e lista dei client in parallelo
		for(j=i;j<nodeList[k].keypointsDetected*7;j=j+7,count++)
		{
			//printf("DISTRIBUTED x: %f LOCAL x: %f \n",nodeList[k].keypoints[j+3],kp[count].pt.x);
			//printf("DISTRIBUTED y: %f LOCAL y: %f \n\n",nodeList[k].keypoints[j+4]+overlap,kp[count].pt.y);
			if(kp[count].pt.x >= nodeList[k].keypoints[j+3]-0.001
				&& kp[count].pt.x <= nodeList[k].keypoints[j+3]+0.001
				&& kp[count].pt.y >= nodeList[k].keypoints[j+4]+overlap-0.001
				&& kp[count].pt.y <= nodeList[k].keypoints[j+4]+overlap+0.001)
			{
				sameKp++;
				//cout<<"Same keypoints, count: "<<count<<endl;
			}
			else
			{
				cout<<"count: "<<count<<endl;
				differentKeypoints.push_back(nodeList[k].keypoints[j+3]+overlap);
				differentKeypoints.push_back(nodeList[k].keypoints[j+4]+overlap);
				differentKp++;
			}
		}
		cout<<"Node: "<<k<<" different keypoints: "<<differentKp<<endl;
		//count è risettato all'ultimo keypoint che matcha coi keypoints dei clients
		count=count-differentKp;
		printf("Punto da cui partono i keypoints del client successivo. x: %f y: %f\n",kp[count].pt.x,kp[count].pt.y);
		k++;
		//Adding overlap
		if(k!=0)
		{
			for(int h=0;h<clientsNumber;h++)
				overlap = overlap + nodeList[h+1].height;
		}
		cout<<"FINDING KEYPOINT OF THE NEXT CLIENT"<<endl;
		//Find the first keypoint of the next client
		for(i=0;i<(nodeList[k].keypointsDetected)*7 && found==false && k<clientsNumber+1;i=i+7)
		{
			//printf("DISTRIBUTED x: %f LOCAL x: %f \n",nodeList[k].keypoints[i+3],kp[count].pt.x);
			//printf("DISTRIBUTED y: %f LOCAL y: %f \n",nodeList[k].keypoints[i+4]+overlap,kp[count].pt.y);
			//printf("%f %f %f %f\n",nodeList[k].keypoints[i+3]-0.001,nodeList[k].keypoints[i+3]+0.001,nodeList[k].keypoints[i+4]-0.001,nodeList[k].keypoints[i+4]+0.001);
			//printf("%f %f \n",kp[count].pt.x,kp[count].pt.y);
			if(kp[count].pt.x >= nodeList[k].keypoints[i+3]-0.001
				&& kp[count].pt.x <= nodeList[k].keypoints[i+3]+0.001
				&& kp[count].pt.y >= nodeList[k].keypoints[i+4]+overlap-0.001
				&& kp[count].pt.y <= nodeList[k].keypoints[i+4]+overlap+0.001)
				found = true;
			else
			{
				differentKeypoints.push_back(nodeList[k].keypoints[i+3]+overlap);
				differentKeypoints.push_back(nodeList[k].keypoints[i+4]+overlap);
			}
		}
		if(k<clientsNumber+1)
		{
			i=i-7;
			found = false;
			cout<<"Indice da dove partono i keypoints giusti: "<<i<<endl;
			differentKp=0;
		}
	}
	//checking different keypoints
	cout<<"Checking different keypoints list. Size: "<<differentKeypoints.size()/2<<endl;
	for(i=count;i<kp.size();i++)
	{
		printf("x: %f y: %f\n",kp[i].pt.x,kp[i].pt.y);
		for(j=0;j<differentKeypoints.size();j=j+2)
		{
			if(kp[i].pt.x >= differentKeypoints[j]-0.001
				&& kp[i].pt.x <= differentKeypoints[j]+0.001
				&& kp[i].pt.y >= differentKeypoints[j+1]-0.001
				&& kp[i].pt.y <= differentKeypoints[j+1]+0.001)
			{
				cout<<"Same"<<endl;
				sameKp++;
			}
		}
	}
	cout<<"Server processing count: "<<count<<" i: "<<i<<endl;
	cout<<"Same keypoints: "<<sameKp<<endl;

	//Checking descriptors
	bool different = false;
	for(k=0;k<clientsNumber+1;k++)
	{
		cout<<"Node "<<k<<" descriptors size: "<<nodeList[k].descriptorsSize<<endl;
		for(j=0;j<nodeList[k].descriptorsSize && different==false;j++,countDescr++)
		{
			if(k==1)
				cout<<(int)nodeList[k].descriptors[j]<<" "<<(int)descriptors.data[countDescr]<<endl;
			if((int)nodeList[k].descriptors[j]==(int)descriptors.data[countDescr])
				sameDescr++;
			else
			{
				different = true;
				while(countDescr%64!=0)
					countDescr--;
			}
		}
		countDescr--;
		cout<<"count descriptors: "<<countDescr<<endl;
		different = false;
		//fare il controllo per trovare quale descrittore è effettivamente giusto (uguale ai keypoints)
	}
	cout<<"Count descriptors: "<<countDescr<<endl;
	cout<<"Same descriptors: "<<sameDescr/64<<endl;
	cout<<"Different descriptors: "<<differentDescr/64<<endl;
	if(sameKp<kp.size())
		return 2;
	*/
	return 0;
}
//Receiving data
void receiveData(int clientsNumber, list_nodes nodeList[])
{
	int k;
	char request;
	int s,dataReceived;
	for(k=0;k<clientsNumber;k++)
	{
		cout<<"Client: "<<nodeList[k+1].ipAddress<<endl;
		s = nodeList[k+1].socket;
		//Receiving total time
		request = 't';
		if(send(s,&request,sizeof(char),0)<0)
			printErrorLog("Request not sent correctly");
		if(recv(s, &nodeList[k+1].time, sizeof(double), 0)<0)
			printErrorLog("Time not received correctly");
		cout<<"Processing time received: "<<nodeList[k+1].time<<"ms"<<endl;
		request='k';
		//Receiving number of keypoints
		if(send(s,&request,sizeof(char),0)<0)
			printErrorLog("Request not sent correctly");
		if(recv(s,&nodeList[k+1].keypointsDetected,sizeof(int), MSG_WAITALL)<0)
			printErrorLog("Keypoints number not received correctly");
		cout<<"Number of keypoints received: "<<nodeList[k+1].keypointsDetected<<endl;
		//Receiving keypoints
		uchar keypoints[nodeList[k+1].keypointsDetected*8];
		dataReceived=recv(s,&keypoints[0],sizeof(char)*8*nodeList[k+1].keypointsDetected, MSG_WAITALL);
		if(dataReceived!=(sizeof(char)*8*nodeList[k+1].keypointsDetected))
			printErrorLog("Keypoints not received correctly");
		//Deserializing keypoints
		unsigned int number;
		float fnumber;
		for(int i=0;i<nodeList[k+1].keypointsDetected*8;i=i+4)
		{
			number = (keypoints[i] << 24);
			number = number + (keypoints[i+1] << 16);
			number = number + (keypoints[i+2] << 8);
			number = number + (keypoints[i+3]);
			fnumber = *((float*)&number);
			nodeList[k+1].keypoints.push_back(fnumber);
			//printf("%f \n",fnumber);
		}
		cout<<"Keypoints: "<<dataReceived/(sizeof(char)*8)<<endl;
		//Receiving descriptors
		request='d';
		if(send(s,&request,sizeof(char),0)<0)
			printErrorLog("Request not sent correctly");
		if(recv(s,&nodeList[k+1].descriptorsSize,sizeof(int),MSG_WAITALL)<0)
			printErrorLog("Descriptors size not received correctly");
		uchar descriptorsArray[nodeList[k+1].descriptorsSize];
		dataReceived = recv(s,&(descriptorsArray[0]),nodeList[k+1].descriptorsSize,MSG_WAITALL);
		if(dataReceived!=nodeList[k+1].descriptorsSize)
			printErrorLog("Descriptors not received correctly");
		for(int i=0;i<nodeList[k+1].descriptorsSize;i++)
		{
			nodeList[k+1].descriptors.push_back(descriptorsArray[i]);
		}
		//cout<<nodeList[k+1].descriptors.size()<<endl;
	}
	//Computing maximum time
	double max = computeMaximumProcessingTime(clientsNumber,nodeList);
	cout<<"DISTRIBUTED PROCESSING TIME: "<<max<<"ms"<<endl;
	//fprintf(file,"%f ",max);
	//Mat serverKp,distributedKPs;
	//drawKeypoints(original,kp,serverKp);
	//imwrite("/home/ubuntu/serverKP.jpg",serverKp,params);
	//drawKeypoints(splitImage[0],distributedKp,distributedKPs);
	//imwrite("/home/ubuntu/distributedKP.jpg",distributedKPs,params);
	//Checking
}
//Returns how many 1 are different
int compareBytes(uchar a, uchar b)
{
	int counter = 0;
	uchar c;
	c = a ^ b;
	while(c>0)
	{
		if(c & 0x01)
			counter++;
		c = c >> 1;
	}
	return counter;
}

